#include "bfs_algorithm.hpp"

std::vector<Point> BFSAlgorithm::search(std::vector<std::vector<int>> &grid, Point start, Point end, cv::Mat &image, int occ_size)
{
  int rows = grid.size();
  int cols = grid[0].size();
  std::vector<std::vector<bool>> visited(rows, std::vector<bool>(cols, false));
  std::vector<std::vector<Point>> parent(rows, std::vector<Point>(cols, {-1, -1}));

  std::queue<Point> q;
  q.push(start);
  visited[start.y][start.x] = true;

  int dx[] = {-1, 0, 1, 0};
  int dy[] = {0, 1, 0, -1};

  while (!q.empty())
  {
    Point current = q.front();
    q.pop();

    if (current.x == end.x && current.y == end.y)
    {
      std::vector<Point> path;
      while (current.x != -1 && current.y != -1)
      {
        path.push_back(current);
        current = parent[current.y][current.x];
      }
      std::reverse(path.begin(), path.end());
      return path;
    }

    for (int i = 0; i < 4; ++i)
    {
      int nx = current.x + dx[i];
      int ny = current.y + dy[i];

      if (nx >= 0 && nx < cols && ny >= 0 && ny < rows && !visited[ny][nx] && grid[ny][nx] == 0)
      {
        q.push({nx, ny});
        visited[ny][nx] = true;
        parent[ny][nx] = current;

        if (!(nx == end.x && ny == end.y))
        {
          cv::Rect roi(nx * occ_size, ny * occ_size, occ_size, occ_size);
          image(roi) = cv::Vec3b(0, 255, 0);
          cv::imshow("BFS Expansion", image);
          cv::waitKey(1);
        }
      }
    }
  }

  return {};
}
